#include <public/capathplanner_iface_v1.h>

// ***********************************************************************

// class EDROOMTopContext

// ***********************************************************************



	// CONSTRUCTORES***********************************************

CAPathPlanner::EDROOMTopContext::EDROOMTopContext (CAPathPlanner &act, CDPath & EDROOMpVarVNextPath, CDLocation & EDROOMpVarVNextStepStartPoing, TEDROOMUInt8 & EDROOMpVarVSimError, CDLocation & EDROOMpVarVTargetLocation, TEDROOMBool & EDROOMpVarVVehicleHalted, CEDROOMPOOLCDPath & EDROOMpPoolCDPath ):

	EDROOMcomponent(act)
	,Msg(EDROOMcomponent.Msg)
	,MsgBack(EDROOMcomponent.MsgBack)
	,TrackPath(EDROOMcomponent.TrackPath)
	,PathPlannerCtrl(EDROOMcomponent.PathPlannerCtrl)
	,Timer(EDROOMcomponent.Timer)
	, VMaxPathSteps ( 255 )
	, VMinPathSteps ( 3 )
	, VNextPath ( EDROOMpVarVNextPath )
	, VNextStepStartPoing ( EDROOMpVarVNextStepStartPoing )
	, VSimError ( EDROOMpVarVSimError )
	, VTargetLocation ( EDROOMpVarVTargetLocation )
	, VVehicleHalted ( EDROOMpVarVVehicleHalted )
	, EDROOMPoolCDPath ( EDROOMpPoolCDPath )
 {

}

CAPathPlanner::EDROOMTopContext::EDROOMTopContext ( EDROOMTopContext &contex ):

	EDROOMcomponent(contex.EDROOMcomponent),
	Msg(contex.Msg),
	MsgBack(contex.MsgBack)
	,TrackPath(contex.TrackPath)
	,PathPlannerCtrl(contex.PathPlannerCtrl)
	,Timer(contex.Timer)
	, VMaxPathSteps ( 255 )
	, VMinPathSteps ( 3 )
	, VNextPath ( contex.VNextPath )
	, VNextStepStartPoing ( contex.VNextStepStartPoing )
	, VSimError ( contex.VSimError )
	, VTargetLocation ( contex.VTargetLocation )
	, VVehicleHalted ( contex.VVehicleHalted )
	, EDROOMPoolCDPath ( contex.EDROOMPoolCDPath )
 {

}

	// EDROOMSearchContextTrans********************************************

bool CAPathPlanner::EDROOMTopContext::EDROOMSearchContextTrans(TEDROOMTransId &edroomCurrentTrans){

	bool edroomValidMsg=false; 

	 switch(Msg->signal){

		 case (SMarkObstacle): 

				if ( *Msg->GetPInterface() == PathPlannerCtrl) {

					edroomCurrentTrans.localId= ObstacleDetected;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

			 break;

		 case (SNewTargetLocation): 

				if ( *Msg->GetPInterface() == PathPlannerCtrl) {

					edroomCurrentTrans.localId= NewTarget;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

			 break;

		 case (SPathRequest): 

				if ( *Msg->GetPInterface() == TrackPath) {

					edroomCurrentTrans.localId= PathReq;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

			 break;

		 case ( EDROOMSignalDestroy ): 

			 edroomValidMsg=true;
			 edroomCurrentTrans.distanceToContext = 0 ;
			 edroomCurrentTrans.localId = -1 ;
			 break;

	}

	return(edroomValidMsg);

}

	// Component Methods  ****************************

void	CAPathPlanner::EDROOMTopContext::FCalPathStep()

{
		print_char('s');
 
 		VNextPath.pathLocations[VNextPath.pathSteps]
					=calNextPathStep(VNextStepStartPoing, VTargetLocation);
 
		VNextPath.pathSteps++;
 
}



void	CAPathPlanner::EDROOMTopContext::FConfigOK()

{
		// reply 
	
	Msg->reply(SConfigOK);
 
 
  VNextStepStartPoing=sensors_get_current_location();
 
  VVehicleHalted=true;	
		
  VNextPath.pathSteps=0;
 
  FProgStepPlanning();
 
 
}



void	CAPathPlanner::EDROOMTopContext::FConfigPlanner()

{
 
	VTargetLocation = *(CDLocation *)Msg->data;
 
	
 
}



void	CAPathPlanner::EDROOMTopContext::FMarkObstacleAndReplanning()

{
 
	CDLocation & varCDLocation = *(CDLocation *)Msg->data;
 
	markObstacle(varCDLocation);
 
 	VVehicleHalted=true;
 
   VNextStepStartPoing=sensors_get_current_location();
   VNextPath.pathSteps=0;
 
}



void	CAPathPlanner::EDROOMTopContext::FMaxSteps()

{
 
 print_char('\n');
 print_char('m');
 
 
}



void	CAPathPlanner::EDROOMTopContext::FProgStepPlanning()

{
 
		// Timing Service 
	 
	Pr_Time	interval(0.1); // Declare time var with an initial value in seconds
	 
	Timer.InformIn( interval, EDROOMprioNormal );
 
 
 
 
}



void	CAPathPlanner::EDROOMTopContext::FSendMinPath()

{
 	FSendPath();
 
 
   FProgStepPlanning();
 
}



void	CAPathPlanner::EDROOMTopContext::FSendPath()

{
	CDPath * ap_CDPath = EDROOMPoolCDPath.AllocData();
	
		// Complete Data 
	
	*ap_CDPath=VNextPath;
	
		// send new Msg
	
	TrackPath.send(STrackPath, EDROOMprioHigh, ap_CDPath, &EDROOMPoolCDPath);
 
   
   VVehicleHalted=false;	
 
	VNextPath.pathSteps=0;
 
 	print_char('\n');
	print_char('\r');
	print_char('x');
   print_char('\n');
   print_char('\r');
 
}



void	CAPathPlanner::EDROOMTopContext::FSetNewTarget()

{
 
 
	VTargetLocation = *(CDLocation *)Msg->data;
 
 	VVehicleHalted=true;	
 	actuators_halt();
 
   VNextStepStartPoing=sensors_get_current_location();
 
	VNextPath.pathSteps=0;
 
}



void	CAPathPlanner::EDROOMTopContext::FSimError()

{
 
 VSimError--;
 
		// reply 
	
	Msg->reply(SConfigError);
 
 
}



TEDROOMBool	CAPathPlanner::EDROOMTopContext::GMaxSteps()

{
 
 
 return (VNextPath.pathSteps==VMaxPathSteps);
 
 
}



TEDROOMBool	CAPathPlanner::EDROOMTopContext::GSendMinPath()

{
 
 
 	return ((VVehicleHalted)&&(VNextPath.pathSteps>=VMinPathSteps));
 
 
}



	//*****Pools

	//CEDROOMPOOLCDPath

CAPathPlanner::EDROOMTopContext::CEDROOMPOOLCDPath::CEDROOMPOOLCDPath(TEDROOMUInt32 elemCount, CDPath* pMem, TEDROOMBool * pMemMarks):CEDROOMProtectedMemoryPool(elemCount, pMem, pMemMarks, sizeof (CDPath)){;}

CDPath *	CAPathPlanner::EDROOMTopContext::CEDROOMPOOLCDPath::AllocData(){
	return(CDPath*)CEDROOMProtectedMemoryPool::AllocData();
}



// ***********************************************************************

// class EDROOMTopState

// ***********************************************************************



	// CONSTRUCTOR*************************************************

CAPathPlanner::EDROOMTopState::EDROOMTopState (CAPathPlanner &act, CEDROOMMemory *pEDROOMMemory  ):
	  EDROOMTopContext( act 
		, VNextPath
		, VNextStepStartPoing
		, VSimError
		, VTargetLocation
		, VVehicleHalted
		, EDROOMPoolCDPath )
	, VSimError ( 1 )
	, VVehicleHalted ( true )
	, EDROOMPoolCDPath ( 3, pEDROOMMemory->poolCDPath, pEDROOMMemory->poolMarkCDPath)
{

}

	// EDROOMBehaviour**********************************************

void CAPathPlanner::EDROOMTopState::EDROOMBehaviour(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans = EDROOMIllegada();

	do{

		switch(edroomCurrentTrans.localId){

			case(Init):
				edroomNextState = Idle;
				break;
			case(Config):
				FConfigPlanner();
				if( VSimError ){
					FSimError();
					edroomCurrentTrans.localId = Config_Error;
					edroomNextState = Idle;
				 } 
				else{
					FConfigOK();
					edroomCurrentTrans.localId = Config_OK;
					edroomNextState = Idle;
				 } 
				break;
			case(CalStep):
				FCalPathStep();
				if( GSendMinPath() ){
					FSendMinPath();
					edroomCurrentTrans.localId = CalStep_SendMinPath;
					edroomNextState = CalculatePath;
				 } 
				else if( GMaxSteps() ){
					FMaxSteps();
					edroomCurrentTrans.localId = CalStep_MaxStep;
					edroomNextState = CalNextPath;
				 } 
				else{
					FProgStepPlanning();
					edroomCurrentTrans.localId = CalStep_NextStep;
					edroomNextState = CalculatePath;
				 } 
				break;
			case(ObstacleDetected):
				FMarkObstacleAndReplanning();
				edroomNextState = CalculatePath;
				break;
			case(NewTarget):
				FSetNewTarget();
				edroomNextState = CalculatePath;
				break;
			case(PathReq):
				FSendPath();
				edroomNextState = CalculatePath;
				break;
			case(Start):
				FProgStepPlanning();
				edroomNextState = CalculatePath;
				break;
		}

		switch(edroomNextState){

			case(I):
				edroomCurrentTrans=EDROOMIllegada();
				break;

			case(Idle):
				edroomCurrentTrans=EDROOMIdlellegada();
				break;

			case(CalculatePath):
				edroomCurrentTrans=EDROOMCalculatePathllegada();
				break;

			case(CalNextPath):
				edroomCurrentTrans=EDROOMCalNextPathllegada();
				FProgStepPlanning();
				break;

		}

		edroomCurrentState=edroomNextState;

	}while(Msg->signal != EDROOMSignalDestroy);

}



	// Inicio**********************************************

void CAPathPlanner::EDROOMTopState::EDROOMInicio(){

edroomCurrentState=I;

}



//	 ***********************************************************************

//	 llegada SubEstado I

//	 ***********************************************************************



TEDROOMTransId CAPathPlanner::EDROOMTopState::EDROOMIllegada(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans.localId = Init;
	edroomCurrentTrans.distanceToContext = 0;
	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja Idle

	// ***********************************************************************



TEDROOMTransId CAPathPlanner::EDROOMTopState::EDROOMIdlellegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (SConfigPathPlanner): 

				 if ( *Msg->GetPInterface() == PathPlannerCtrl) {

					edroomCurrentTrans.localId = Config;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

			case (SStartPlanning): 

				 if ( *Msg->GetPInterface() == PathPlannerCtrl) {

					edroomCurrentTrans.localId= Start;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja CalculatePath

	// ***********************************************************************



TEDROOMTransId CAPathPlanner::EDROOMTopState::EDROOMCalculatePathllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (EDROOMSignalTimeout): 

				 if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId = CalStep;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja CalNextPath

	// ***********************************************************************



TEDROOMTransId CAPathPlanner::EDROOMTopState::EDROOMCalNextPathllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	return(edroomCurrentTrans);

}



